#include "Int_MT6816.h"

#define MT6816_CS GPIO_Pin_4
#define MT6816_Z GPIO_Pin_5

// 设备参数
#define DEVICE_CIRCUMFERENCE 1.0f // 每转1米
#define ENCODER_4X_PPR 4096       // 4倍频后每转计数

// 全局变量
// 工作模式枚举
typedef enum
{
    ENCODER_MODE_STOP_WAKEUP, // STOP模式，等待唤醒
    ENCODER_MODE_NORMAL_WORK  // 正常工作模式
} EncoderWorkMode_t;

// 全局变量
static EncoderWorkMode_t encoder_work_mode = ENCODER_MODE_NORMAL_WORK;
uint8_t wakeup_flag = 0;                 // 唤醒标志
static uint16_t saved_encoder_count = 0; // 保存的编码器计数值
static float saved_displacement = 0.0f;  // 保存的位移值

static float current_displacement = 0.0f; // 当前位移（米，始终≥0）
static uint16_t last_encoder_count = 0;   // 上次编码器读数
static int16_t diff = 0;                  // 定时器计数差值
float adjusted_displacement = 0.0f;       // 修正值
static uint8_t rotation_direction = 0;    // 旋转方向状态
void Int_MT6816_Init()
{
    // 开始TIM3时钟
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    // 开启GPIOA时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);
    // 使能AFIO时钟
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);

    // GPIO初始化
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_6 | GPIO_Pin_7);
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_InitStructure.GPIO_Pin = MT6816_CS;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // 中断配置
    GPIO_InitStructure.GPIO_Pin = MT6816_Z;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // 将引脚连接到EXTI线
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource5);

    // 配置外部中断
    EXTI_InitTypeDef EXTI_InitStructure;
    EXTI_InitStructure.EXTI_Line = EXTI_Line5;
    EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
    EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising;
    EXTI_InitStructure.EXTI_LineCmd = ENABLE;
    EXTI_Init(&EXTI_InitStructure);

    // 配置NVIC
    NVIC_InitTypeDef NVIC_InitStructure;
    NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    // 默认拉高CS
    GPIO_SetBits(GPIOA, MT6816_CS);

    // 时基单元初始化
    TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseStructure.TIM_Period = 65536 - 1;
    TIM_TimeBaseStructure.TIM_Prescaler = 1 - 1;
    TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);

    // 输入捕获初始化
    TIM_ICInitTypeDef TIM_ICInitStructure;
    TIM_ICStructInit(&TIM_ICInitStructure);

    TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
    TIM_ICInitStructure.TIM_ICFilter = 0x0F;
    TIM_ICInit(TIM3, &TIM_ICInitStructure);

    TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
    TIM_ICInitStructure.TIM_ICFilter = 0x0F;
    TIM_ICInit(TIM3, &TIM_ICInitStructure);

    // 编码器接口设置 4倍频
    TIM_EncoderInterfaceConfig(TIM3, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Falling);

    TIM_Cmd(TIM3, ENABLE);
}

// 添加函数声明，用于中断处理程序更新方向 可用于校准
void Int_MT6816_UpdateDirection(void)
{
    // 在Z相上升沿时刻读取A/B状态
    uint8_t a_level = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6);
    uint8_t b_level = GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7);

    if (a_level == 0 && b_level == 1)
    {
        // 正转: 线变长
        rotation_direction = 1;
    }
    else
    {
        // 反转或其他状态
        rotation_direction = 0;
    }

}

// 更新位移计算
static void UpdateDisplacement(void)
{
    uint16_t current_count = TIM_GetCounter(TIM3);

    // 计算计数器变化
    diff = current_count - last_encoder_count;

    if (diff > 32767)
    {
        diff -= 65536; // 反向溢出
    }
    else if (diff < -32768)
    {
        diff += 65536; // 正向溢出
    }

    // 计算位移变化量
    float displacement_change = (float)diff / ENCODER_4X_PPR * DEVICE_CIRCUMFERENCE;

    // 更新当前位移
    current_displacement += displacement_change;

    // 确保位移不会小于0
    if (current_displacement < 0.0f)
    {
        current_displacement = 0.0f;
    }

    // 更新上次计数
    last_encoder_count = current_count;
}

// 获取当前位移
float Int_MT6816_GetCurrentDisplacement(void)
{
    UpdateDisplacement();
    return current_displacement;
}

// 获取详细信息
void GetDisplacementInfo(float *displacement, int32_t *full_revs, float *partial_rev, float calibration)
{
    UpdateDisplacement();
    float adjusted_displacement = current_displacement - calibration;

    // 如果调整后的值小于0，返回0
    if (adjusted_displacement < 0.0f)
    {
        adjusted_displacement = 0.0f;
    }
    *displacement = adjusted_displacement;
    *full_revs = (int32_t)(current_displacement / DEVICE_CIRCUMFERENCE);       // 完整圈数
    *partial_rev = current_displacement - (*full_revs * DEVICE_CIRCUMFERENCE); // 不足一圈的部分
}

uint8_t Int_MT6816_GetDirectionByC(void)
{
    return rotation_direction;
}

// 手动设置当前位移（校准用）
void SetCurrentDisplacement(float displacement)
{
    if (displacement >= 0.0f)
    {
        current_displacement = displacement;
    }
}

// 重置到原点
void ResetToOrigin(void)
{
    current_displacement = 0.0f;
    last_encoder_count = TIM_GetCounter(TIM3);
}
// 进入STOP模式配置
void Int_MT6816_EnterStopMode(void)
{
    if (encoder_work_mode == ENCODER_MODE_STOP_WAKEUP)
    {
        return; // 已经在STOP模式
    }

    // 保存当前状态
    saved_encoder_count = TIM_GetCounter(TIM3);
    saved_displacement = current_displacement;

    // 禁用TIM3
    TIM_Cmd(TIM3, DISABLE);

    // 配置PA6(A相)为外部中断
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // 配置PA7(B相)为外部中断
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // 配置EXTI6 (A相)
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource6);
    EXTI_InitTypeDef EXTI_InitStructure;
    EXTI_InitStructure.EXTI_Line = EXTI_Line6;
    EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt;
    EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
    EXTI_InitStructure.EXTI_LineCmd = ENABLE;
    EXTI_Init(&EXTI_InitStructure);

    // 配置EXTI7 (B相)
    GPIO_EXTILineConfig(GPIO_PortSourceGPIOA, GPIO_PinSource7);
    EXTI_InitStructure.EXTI_Line = EXTI_Line7;
    EXTI_Init(&EXTI_InitStructure);

    // 配置NVIC
    NVIC_InitTypeDef NVIC_InitStructure;
    NVIC_InitStructure.NVIC_IRQChannel = EXTI9_5_IRQn;
    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
    NVIC_Init(&NVIC_InitStructure);

    // 清除唤醒标志
    wakeup_flag = 0;
    encoder_work_mode = ENCODER_MODE_STOP_WAKEUP;
}
// 退出STOP模式配置
void Int_MT6816_ExitStopMode(void)
{
    if (encoder_work_mode == ENCODER_MODE_NORMAL_WORK)
    {
        return; // 已经在正常模式
    }

    // 禁用A/B相外部中断
    EXTI_InitTypeDef EXTI_InitStructure;
    EXTI_InitStructure.EXTI_Line = EXTI_Line6;
    EXTI_InitStructure.EXTI_LineCmd = DISABLE;
    EXTI_Init(&EXTI_InitStructure);

    EXTI_InitStructure.EXTI_Line = EXTI_Line7;
    EXTI_Init(&EXTI_InitStructure);

    // 重新配置PA6/PA7为编码器输入
    GPIO_InitTypeDef GPIO_InitStructure;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
    GPIO_InitStructure.GPIO_Pin = (GPIO_Pin_6 | GPIO_Pin_7);
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    // 恢复TIM3编码器配置
    TIM_SetCounter(TIM3, saved_encoder_count);
    current_displacement = saved_displacement;
    last_encoder_count = saved_encoder_count;

    // 重新启用TIM3
    TIM_Cmd(TIM3, ENABLE);

    encoder_work_mode = ENCODER_MODE_NORMAL_WORK;
}
// 检查是否从STOP模式唤醒
uint8_t Int_MT6816_IsWakeupFromStop(void)
{
    return wakeup_flag;
}
// 清除唤醒标志
void Int_MT6816_ClearWakeupFlag(void)
{
    wakeup_flag = 0;
}

uint8_t Int_MT6816_GetStatus(void)
{
    if (diff > 0)
    {
        return 1;
    }
    else if (diff < 0)
    {
        return 2;
    }
    else
    {
        return 0;
    }
}
